#include "AIAgentGroup.h"
#include <assert.h>
#include "AIAgent.h"

void AIAgentGroup::AddAgent(AIAgent* agent)
{
	agent->SetGroup(this);
	m_agents.push_back(agent);
}

std::vector<AIAgent*> AIAgentGroup::GetNeighbours(AIAgent* agent, float radius)
{
	std::vector<AIAgent*> agentsVec;
	float sqRadius = radius * radius;

	//assert(m_agents.size());
	//Loop over all agents
	int size = m_agents.size();
	for( int i = 0; i < size; i++)
	{
		if(m_agents[i] == agent)
		{
			continue;
		}
		//Get dist from 'agent' to current Agent
		float sqDist = (agent->GetPos() - m_agents[i]->GetPos()).LengthSqr();

		//If less than sqRadius then add agent to result vector
		if(sqDist < sqRadius)
		{
			agentsVec.push_back(m_agents[i]);
		}
	}
	return agentsVec;
}